/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/status.cpp
 * @Description  : 实现了WorkStatusRecord类，用于管理和记录工作状态，包括静态和动态位置的设置、路径和节点的查找、以及其他状态信息的更新。该类依赖于World对象来获取路径和节点信息，并通过与agv_msgs::msg::EdgeState和ProjectData等数据结构的交互来实现状态记录和导航功能。
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:04:15
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "status.h"
#include "tools.h"
#include "public_kernel.h" 

constexpr float MAX_OUTSIDE_DIST = 0.1f;
constexpr float LAZYS_TRAJ_RATIO = 3.0f;
constexpr float MAX_AUTOLINE_DISTANCE = 20.0f;
constexpr float MAX_AUTOLINE_ANGLE  = PI / 18;

//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "CWorkStatusRecord".

//
//   The default constructor.
//
WorkStatusRecord::WorkStatusRecord(World* world) : world_(world) 
{
    Clear();
}


//
//   Initialize the object.
//
void WorkStatusRecord::Init(World* world, int level) 
{
  world_ = world;
  user_param_level_ = level;
}

//
//   Clear the status.
//
void WorkStatusRecord::Clear() 
{
  loc_type_ = TagStateType::LOCATION_UNKNOWN;
  level_ = user_param_level_;
  last_is_op_ = false;
  event_ = -1;
  task_id_ = 65535;
  sub_task_id_ = 65535;
  task_status_ = TaskStatus::UnKnown;
  from_node_ = -1;
  to_node_ = -1;
  equipment_status_ = 0;
  quality_ = 0.0f;
  is_idle_ = true;
  rack_follow_up_ = 0;
  rcs_fb_.Clear();
  to_run_blink_ = false;
}

//
// 寻找当前路径或节点的路径bit属性
//

uint16_t WorkStatusRecord::FindCurBits()
{
    uint16_t bits = 0;
    // 地图是否存在
    if (!world_) {
        DEBUG_ERROR_OUT("failed: when find cur FindCurBits(m_pWorld is null)");
        return bits;
    }

    // 节点是否有效
    if (from_node_ == NULLID || to_node_ == NULLID) {
        DEBUG_ERROR_OUT("failed: when find cur FindCurBits(nodeId has 65535)");
        return bits;
    }

    // 是否在节点上
    if (from_node_ == to_node_) {

        Node *node  = world_->GetNode(from_node_);
        if (!node) {
            DEBUG_ERROR_OUT("failed: when find cur FindCurBits nodeId is" << from_node_);
            return bits;
        }
        bits = node->Bits();
        DEBUG_OUT("Ok: when find FindCurBits nodeId is" << from_node_ << ",bits=" << (int)(bits));
        return bits;
    }

    // 是否在路径上
    Path *path  = world_->GetPathPointer(from_node_, to_node_);
    if (!path) {
        DEBUG_ERROR_OUT("find cur path is null when find cur FindCurBits "
                        << ",FromNode:" << from_node_ << ",ToNode:" << to_node_);
        return bits;
    }

    bits = path->Bits();
    DEBUG_OUT("Ok: when find FindCurBits path bits is" << (int)(bits) << ",FromNode:" << from_node_ << ",ToNode:" << to_node_);
    return bits;
}

//
//
//
agv_msgs::msg::EdgeState WorkStatusRecord::FindCurEdge()
{
    static agv_msgs::msg::EdgeState cur_edge_state;
    static USHORT from_node_record = NULLID, to_node_record = NULLID;

    if (!world_) {
        DEBUG_OUT("failed: cur world is null when find cur edge"
                  << " uFromNodeRecord:" << from_node_record
                  << ",uToNodeRecord:" << to_node_record << ",FromNode:" << from_node_ << ",ToNode:" << to_node_);
        cur_edge_state = agv_msgs::msg::EdgeState();
        from_node_record = NULLID;
        to_node_record = NULLID;
        return cur_edge_state;
    }

    if (from_node_record == from_node_ && to_node_record == to_node_) {
        return cur_edge_state;
    }

    // 节点清除(如果手动模式切换后，需要清除静态局部变量)
    if (from_node_ == NULLID || to_node_ == NULLID) {
        DEBUG_OUT("failed: when find cur edge(nodeId has 65535)"
                  << " uFromNodeRecord:" << from_node_record
                  << ",uToNodeRecord:" << to_node_record << ",FromNode:" << from_node_ << ",ToNode:" << to_node_);

        from_node_record = NULLID;
        to_node_record = NULLID;
        cur_edge_state = agv_msgs::msg::EdgeState();
        return cur_edge_state;
    }

    // 更新有效新的初始化节点
     from_node_record = from_node_;
    to_node_record = to_node_;

    Path *path = world_->GetPathPointer(from_node_, to_node_);
    if (!path) {
        from_node_record = NULLID;
        to_node_record = NULLID;
        cur_edge_state = agv_msgs::msg::EdgeState();
        DEBUG_OUT("find cur path is null when find cur edge "
                  << ",FromNode:" << from_node_ << ",ToNode:" << to_node_);
        return cur_edge_state;
    }

    try {
        cur_edge_state.edgeid = std::to_string(path->id_);
        cur_edge_state.released = false;
        cur_edge_state.sequenceid = 0;
        cur_edge_state.edgedescription = "";
        DEBUG_OUT("ok: find cur edge ok path id=" << path->id_);
    }
    catch (...)
    {
        DEBUG_OUT("error: find cur edge failed path id=" << path->id_);
    }

    return cur_edge_state;
}

//
//   Set the vehicle's static location (form #2).
//
//
//   Set the vehicle's static location (form #2).
//
bool WorkStatusRecord::StaticLocation(unsigned short at_node, unsigned short aux_node,
                                        bool update_end_node, bool use_map_theory_data)
{
    ASSERT(world_ != NULL);
    if (!world_) return false;

    if (!world_->IsNode(at_node)) return false;

    loc_type_ = TagStateType::STATIC_LOCATION;
    DEBUG_OUT("StaticLocation:m_nLocType(STATIC_LOCATION:1)=" << static_cast<int>(loc_type_)
                                                              << ",uAtNode=" << at_node << ",uToNode=" << aux_node);

    Path* path = world_->GetPathPointer(at_node, aux_node);
    if (!path) return false;

    if (update_end_node) {
        from_node_ = to_node_ = aux_node;
        abs_progress_ = 0;
        Node* node = world_->GetNode(aux_node);
        if (!node)         {
            DEBUG_ERROR_OUT("error: StaticLocation failed; pNode is null;uToNode=" << aux_node);
            return false;
        }

        if (use_map_theory_data) {
            cur_posture_.x_ = node->x_;
            cur_posture_.y_ = node->y_;
            cur_posture_.theta_ = path->GetHeading(*node).radian_;
        }
        // m_pToNode = pNode;
    } else {
        from_node_ = to_node_ = at_node;
        abs_progress_ = 0;
        Node* node = world_->GetNode(at_node);
        if (!node) {
            DEBUG_ERROR_OUT("error: StaticLocation failed; pNode is null;uAtNode=" << at_node);
            return false;
        }

        if (use_map_theory_data) {
            cur_posture_.x_ = node->x_;
            cur_posture_.y_ = node->y_;
            cur_posture_.theta_ = path->GetHeading(*node).radian_;
        }
        // m_pToNode = pNode;
    }
    return true;
}

//
//   Set the vehicle's static location (form #3).
//
bool WorkStatusRecord::StaticLocation(unsigned short at_node, Posture& posture) 
{
    ASSERT(world_ != nullptr);
    if (!world_) return false;
    if (!world_->IsNode(at_node)) return false;

    loc_type_ = TagStateType::STATIC_LOCATION;
    from_node_ = to_node_ = at_node;
    abs_progress_ = 0;
    cur_posture_ = posture;
    DEBUG_OUT("StaticLocation at node :cur pst x:" << cur_posture_.x_ << " y:" << cur_posture_.y_ << " t:" << cur_posture_.theta_);
    return false;
}

//
//   Set the vehicle's static location.
//
bool WorkStatusRecord::StaticLocation(unsigned short at_node, Angle heading, 
                                      MoveDir wheel_move_dir, Angle steer, short level) 
{
    if (world_ == nullptr) return false; 
    if (!world_->IsNode(at_node)) return false;

    loc_type_ = TagStateType::STATIC_LOCATION;
    from_node_ = to_node_ = at_node;
    abs_progress_ = 0;
    cur_posture_.theta_ = heading.radian_;
    wheel_move_dir_ = wheel_move_dir;
    steer_angle_ = steer;
    last_is_op_ = false;
    level_ = level;
    return true;
}

//
//   special task node/middle to middle: Set the vehicle's dynamic location.
//
bool WorkStatusRecord::DynamicLocation(unsigned short from_node, unsigned short to_node, 
                                       float abs_progress, Angle heading, 
                                       MoveDir wheel_move_dir, Angle steer, short level) 
{
    if (!world_) return false; 
    loc_type_ = TagStateType::DYNAMIC_LOCATION;
    from_node_ = from_node;
    to_node_ = to_node;
    Path* path = world_->GetPathPointer(from_node, to_node);

    if (!path) return false; 
    traj_type_ = path->type_;
    abs_progress_ = abs_progress;
    cur_posture_.theta_ = heading.radian_;
    wheel_move_dir_ = wheel_move_dir;
    steer_angle_ = steer;
    last_is_op_ = false;
    level_ = level;
    return true;
}

// Posture CWorkStatusRecord::GetEndNodePosture()
// {
//     Posture pst;
//     Path *pPath = m_pWorld->GetPathPointer(m_uFromNode, m_uToNode);
//     if (!pPath)
//     {
//         DEBUG_ERROR_OUT("error:GetEndNodePosture pPath is null");
//         return pst;
//     }
//     pst.x = pPath->GetEndNode().x;
//     pst.y = pPath->GetEndNode().y;
//     pst.fThita = pPath->GetEndNode().m_fHeading; // degree change by qf     // pPath->GetEndNode().m_fHeading;
//     return pst;
// }

//
//   Set the vehicle's dynamic location.
//
//   Note:
//   This function is only used in dynamic navigation, so it does not provide
//   fuzzy checking for nodes.
//
ProjectData* WorkStatusRecord::FindLocByPosture(Posture& posture, Path* ref_path) 
{
    ASSERT(world_ != nullptr);
    if (world_ == nullptr) return nullptr;

    if (world_->GetProjectData(posture, ref_path, &project_data_)) {
        loc_type_ = TagStateType::DYNAMIC_LOCATION;
        from_node_ = ref_path->start_node_;
        to_node_ = ref_path->end_node_;
        traj_type_ = ref_path->type_;
        abs_progress_ = project_data_.relative_progress_;
        nav_level_ = 2;
        cur_posture_ = posture;
        DEBUG_OUT("init pos: startNode=" << from_node_ << ", endNode=" << to_node_ 
                  << ", prog=" << abs_progress_ << ", trajType=" << traj_type_);
        DEBUG_OUT("FindLocByPosture: curPos: x, y, theta=" << cur_posture_.x_ 
                  << " " << cur_posture_.y_ << " " << cur_posture_.theta_);
        return &project_data_;
    }
    return nullptr;
}

//
//   Set the vehicle's dynamic location.
//
//   Note:
//     This function is mainly used in initial location calibration,  so it provides
//     fuzzy checking for nodes.
//     FXIME:  m_fAbsProgress is equal to  m_ProjectData.fRelativeProgress(from:0 to:-1)
//
ProjectData* WorkStatusRecord::FindLocByPosture(Posture& posture, float max_dev_x, float max_dev_theta) 
{
    uint16_t path_index = 0xFFFF;
    ProjectData smaller_project_data;
    bool result = false;
    assert(world_ != nullptr);
    if (!world_) return nullptr;
    for (uint16_t i = 0; i < world_->PathBase::path_count_; i++) {
        if (world_->GetProjectData(posture, world_->path_index_[i].ptr_, &smaller_project_data)) {
            if (path_index == 0xFFFF || smaller_project_data < project_data_) {
                project_data_ = smaller_project_data;
                path_index = i;
                result = true;
                DEBUG_OUT("FindLocByPosture: pathIndex=" << path_index);
            }
        }
    }

    // if (!bResult)
    //     return NULL;

    bool init_dev_ok = false;
    Path* path = nullptr;
    if (result) {
        // find ok cur index is pathIndex
        path = world_->path_index_[path_index].ptr_;
        bool dev_ok = fabs(project_data_.x_) < max_dev_x;
        bool angle_ok = abs(Angle(project_data_.theta_)) < Angle(max_dev_theta);
        // Check whether the deviations are within permitted ranges
        init_dev_ok = dev_ok && angle_ok;
    }

    // add by qf at 2022-12-7
    Node near_node;
    if (!init_dev_ok) {
        bool ret = world_->FindNearNodeByXy(posture, near_node, 0.15f);
        if (ret) {
            StaticLocation(near_node.id_, posture);
            project_data_.x_ = 0.06f;
            project_data_.theta_ = 0;
            project_data_.relative_progress_ = 0.0f;
            DEBUG_OUT("##00 FindNearNodeByXy static:, curPos: x,y,theta=" << cur_posture_.x_ << " " << cur_posture_.y_ << " " << cur_posture_.theta_ << " ");
            return &project_data_;
        }
        DEBUG_OUT("##0 find path failed; dev is too long;"
                  << "dev:"
                  << project_data_.x_ << " "
                  << ",ftheta:"
                  << project_data_.theta_);
        loc_type_ = TagStateType::STANDBY_LOCATION;

        return nullptr;
    }

    if (!path) {
        DEBUG_OUT("FindLocByPosture path is null");
        return nullptr;
    }

    // if (!bInitDevOK)
    // {
    //     DEBUG_OUT("##0 find path failed; dev is too long;"
    //               << "dev:"
    //               << m_ProjectData.fX << " "
    //               << ",ftheta:"
    //               << m_ProjectData.fThita);
    //     m_nLocType = STANDBY_LOCATION;
    //     return NULL;
    // }

    // change by qf
    float abs_progress = project_data_.relative_progress_;
    bool near_start_node = abs_progress < path->FuzzyNodeCheckLimit();
    bool near_end_node = fabs(path->Size() - abs_progress) < path->FuzzyNodeCheckLimit();

    if (near_start_node && init_dev_ok) {
        // StaticLocation(pPath->m_uStartNode, pPath->m_uEndNode, false);
        StaticLocation(path->start_node_, posture);
        DEBUG_OUT("##1 NearStartNode static:, curPos: x,y,thita=" << cur_posture_.x_ << " " << cur_posture_.y_ << " " << cur_posture_.theta_ << " ");
    } else if (near_end_node && init_dev_ok) {
        // StaticLocation(pPath->m_uStartNode, pPath->m_uEndNode, true);
        StaticLocation(path->end_node_, posture);
        DEBUG_OUT("##2 bNearEndNode static:, curPos: x,y,thita=" << cur_posture_.x_ << " " << cur_posture_.y_ << " " << cur_posture_.theta_ << " ");
    } else {
        //        if (bInitDevOK)
        //            m_nLocType = DYNAMIC_LOCATION;
        //        else
        //            m_nLocType = STANDBY_LOCATION;
        loc_type_ = TagStateType::DYNAMIC_LOCATION;
        from_node_ = path->start_node_;
        to_node_ = path->end_node_;
        traj_type_ = path->type_;
        deviation_progress_ = 0;
        abs_progress_ = abs_progress;
        dev_x_ = project_data_.x_;
        dev_theta_ = project_data_.theta_;
        cur_posture_ = posture;
        DEBUG_OUT("##3init pos: startNode=" << from_node_ << ",endNode=" << to_node_ << ",prog=" << abs_progress_ << ",trajType=" << traj_type_);

        DEBUG_OUT("##3 FindLocByPosture dynatic pos:, curPos: x,y,thita=" << cur_posture_.x_ << " " << cur_posture_.y_ << " " << cur_posture_.theta_ << " ");
    }
    nav_level_ = 2;

    DEBUG_OUT("locType:" << (int)loc_type_
                         << ",fromNode:" << from_node_
                         << ",ToNode:" << to_node_
                         << ",trajType:" << traj_type_
                         << ",absProgress:" << abs_progress_
                         << ",dev:" << dev_x_
                         << ",devThita:" << dev_theta_
                         << ",devProg:"<<deviation_progress_);

    return &project_data_;
}

//
//   Assign a new velocity level.
//
bool WorkStatusRecord::SetLevel(int16_t level) {
    if (level > 9 || level < 0) {
        return false;
    }
    level_ = level;
    return true;
}
int WorkStatusRecord::GetQrCodeId(uint16_t id) {
    assert(world_ != nullptr);
    if (world_ == nullptr) {
        return -1;
    }
    if (!world_->IsNode(id)) {
        return -1;
    }
    Node* node = world_->GetNode(id);
    if (node) {
        int qr_id = node->GetQrId();
        return qr_id;
    }
    return -1;
}
//
// 原地矫正后，强制更新当前位置
//
void WorkStatusRecord::UpdateCurPos(const Posture& postrue) 
{
    DEBUG_OUT("when adjust agv:UpdataCurPos:"
              << ",last Pos.x=" << cur_posture_.x_ << ",Pos.y=" << cur_posture_.y_ << ",Pos.thita=" << cur_posture_.theta_
              << ",New Pos.x=" << postrue.x_ << ",pos.y=" << postrue.y_ << ",pos.thita=" << postrue.theta_);

    cur_posture_ = postrue;
}

bool WorkStatusRecord::LoadFromSqlite() 
{
    //    BOOL bRet = 0; //JsonRecord::getObj().readData(record);
    //    if(!bRet || record.bValid != 1){
    //        return FALSE;
    //    }
    return FALSE;

    //    m_nLocType   = record.locType;
    //    m_uFromNode  = record.startNode;
    //    m_uToNode    = record.endNode;
    //    m_nEquipmentStatus = record.bLoad;
    //    m_fProgPercent = record.progPercent;
    //    m_fProgress   = record.progress;
    //    //FIXME: m_recordPst to change m_curPst
    //    m_curPst.x = record.x;
    //    m_curPst.y = record.y;
    //    m_curPst.fThita = record.angle;

    DEBUG_OUT("fromNode:" << from_node_
                          << ",toNode:" << to_node_
                          << ",locType:" << (int)loc_type_
                          << ",load:" << equipment_status_
                          << ",progPercent:" << prog_percent_
                          << ",absProgress:" << abs_progress_);

    // Node *pNode = m_pWorld->GetNode(m_uToNode);
    // m_pToNode = pNode;

    switch (loc_type_) {
        case TagStateType::LOCATION_UNKNOWN:
        case TagStateType::STANDBY_LOCATION:
            abs_progress_ = 0;
            break;

        case TagStateType::STATIC_LOCATION:
            abs_progress_ = 0;
            if (!world_->GetPosture(from_node_, from_node_, abs_progress_, cur_posture_))
            {
                loc_type_ = TagStateType::LOCATION_UNKNOWN;
                abs_progress_ = 0;
            }
            break;

        case TagStateType::DYNAMIC_LOCATION:
        {
            Path *path = world_->GetPathPointer(from_node_, to_node_);
            if (path != NULL)
            {
                if (!world_->GetPosture(from_node_, to_node_, abs_progress_, cur_posture_))
                {
                    loc_type_ = TagStateType::LOCATION_UNKNOWN;
                    abs_progress_ = 0;
                }
            }
            else
            {
                loc_type_ = TagStateType::LOCATION_UNKNOWN;
                abs_progress_ = 0;
            }
        }
        break;
    }

    return TRUE;
}

///////////////////////暂时不启用函数接口////////////////////////
bool WorkStatusRecord::ComputeDisFromPointToLineCenter(const Posture& posture, Path* path, float& distance) 
{
    return false;
    // CPnt ptStart = pPath->GetStartPnt();
    // CPnt ptEnd = pPath->GetEndPnt();
    // if ((pPath == NULL) || (ptStart == ptEnd))
    //     return FALSE;

    // float fX0 = pst.x;
    // float fY0 = pst.y;
    // float fX1 = ptStart.x;
    // float fY1 = ptStart.y;
    // float fX2 = ptEnd.x;
    // float fY2 = ptEnd.y;
    // float fCenterX = (fX1 + fX2) / 2;
    // float fCenterY = (fY1 + fY2) / 2;

    // fDis = sqrt((fX0 - fCenterX) * (fX0 - fCenterX) + (fY0 - fCenterY) * (fY0 - fCenterY));
    // return TRUE;
}

//
// Two straight lines intersect vertically to find the shortest distance and intersection point
// y = k1x + b1
// y = k2x + b2
//
bool WorkStatusRecord::SearchGoodLazySPath(Posture& posture, Path* path, float& rate, Point& end_point) 
{
    return false;
    // CPnt ptStart = pPath->GetStartPnt();
    // CPnt ptEnd = pPath->GetEndPnt();

    // if ((pPath == NULL) || (ptStart == ptEnd))
    //     return FALSE;

    // float fNodeToLineDis;
    // float fToStartNodeDis; // useless
    // float fToEndNodeDis;
    // float fToCenterNodeDis;
    // float fMinLength;

    // float fX0 = pst.x;
    // float fY0 = pst.y;
    // float fX1 = ptStart.x;
    // float fY1 = ptStart.y;
    // float fX2 = ptEnd.x;
    // float fY2 = ptEnd.y;
    // float fX3, fY3; //(x3,y3) Vertical intersection of two lines
    // float fX4, fY4; // center point of the path

    // float fA = fY2 - fY1;
    // float fB = fX1 - fX2;
    // float fC = fX1 * (fY1 - fY2) + fY1 * (fX2 - fX1); // Simplification denominator: calc b; b = y1 - [x1(y2-y1)/(x2-x1)]

    // // qf
    // bool bEqualXAxis = fabs(fB) < 0.000001f;
    // bool bEqualYAxis = fabs(fA) < 0.000001f;
    // if (bEqualXAxis && !bEqualYAxis) // if((fB == 0) && (fA !=0)) //Parallel to Y axis
    // {
    //     fX3 = -1.0f * fC / fA; // x1 or x2
    //     fY3 = fY0;
    //     fNodeToLineDis = fabsf(fX0 + fC / fA);
    // }
    // else if (!bEqualXAxis && bEqualYAxis) // else if((fB != 0) && (fA ==0))// Parallel to X axis
    // {
    //     fX3 = fX0;
    //     fY3 = -1.0 * fC / fB; // y = b = -c/b; k =0
    //     fNodeToLineDis = fabsf(fY0 + fC / fB);
    // }
    // else
    // {
    //     float fK1 = -1.0f * fA / fB;
    //     float fb1 = -1.0f * fC / fB;
    //     float fK2 = fB / fA;
    //     float fb2 = fY0 - fK2 * fX0;

    //     fY3 = (fb2 * fK1 - fb1 * fK2) / (fK1 - fK2);
    //     fX3 = (fY3 - fb1) / fK1;

    //     fNodeToLineDis = fabsf(fA * fX0 + fB * fY0 + fC) / ((float)sqrt(fA * fA + fB * fB));
    // }

    // fX4 = (fX1 + fX2) / 2;
    // fY4 = (fY1 + fY2) / 2;
    // fToStartNodeDis = sqrt((fX1 - fX3) * (fX1 - fX3) + (fY1 - fY3) * (fY1 - fY3));
    // fToCenterNodeDis = sqrt((fX4 - fX3) * (fX4 - fX3) + (fY4 - fY3) * (fY4 - fY3));
    // fToEndNodeDis = sqrt((fX2 - fX3) * (fX2 - fX3) + (fY2 - fY3) * (fY2 - fY3));
    // fMinLength = fNodeToLineDis * LAZYS_TRAJ_RATIO;

    // (void)(fToStartNodeDis);

    // if (fabsf(fToCenterNodeDis - fToEndNodeDis - (pPath->m_fSize) / 2) <= 0.03f) // 在终点一侧
    //     return FALSE;
    // else if (fabsf(fToCenterNodeDis + fToEndNodeDis - (pPath->m_fSize) / 2) <= 0.03f) // 在中心到终点之间
    //     return FALSE;
    // else if (fToCenterNodeDis < (fMinLength + 0.03)) // 其他情况
    //     return FALSE;

    // fRate = fToCenterNodeDis;
    // stEndPnt.x = fX4;
    // stEndPnt.y = fY4;

    // return TRUE;
}

double WorkStatusRecord::Distance(double x1, double y1, double x2, double y2)
{
    return (double)sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
}

//
// S: fLazySRate
// SPIN: fSpinRate(spinRad)
//
int WorkStatusRecord::SearchBestPathByPosture(Posture& posture, float& lazy_s_rate, Angle& angle,
                                Point& end_point, Angle& start_angle, Angle& end_angle,
                                TurnDir& turn_dir, float& spin_radius)

{
    return (int)SearchBestPathStep::STEP_FAILED;
    // USHORT pathIndex = 0xFFFF;
    // int nResult = STEP_FAILED;
    // float fMinDis;
    // float fMinRate;
    // float fDis;
    // Path *pTempPath;
    // Path *pPath;
    // float fTempRate;
    // CPnt ptTempEnd;
    // BOOL bGoodLazySPath;
    // BOOL bGoodSpinPath;
    // BOOL bGoodDis;
    // Angle angStartTemp;
    // Angle angEndTemp;
    // CTurnDir TurnDirTemp;
    // int nStartStep = STEP_INIT;

    // ASSERT(m_pWorld != NULL);
    // if (m_pWorld == NULL)
    // {
    //     return nResult;
    // }

    // for (USHORT i = 0; i < m_pWorld->PathBase::m_uCount; i++)
    // {
    //     pTempPath = m_pWorld->m_pPathIdx[i].m_ptr;
    //     if (pTempPath->m_uType != LINE_TYPE)
    //         continue;

    //     Node nd = pTempPath->GetStartNode(); // line path GetHeading(nd) is useless
    //     Angle lineAng = pTempPath->GetHeading(nd);

    //     float fAngleDiff = fabsf(ptStart.fThita - lineAng.m_fRad);
    //     fAngleDiff = Angle::NormAngle2(fAngleDiff); // fabsf->[-pi,pi]
    //     //        while(fAngleDiff >PI){
    //     //            fAngleDiff -= 2*PI;
    //     //        }

    //     if (fabsf(fAngleDiff) > PI_2 /*PI/2*/)
    //         continue;

    //     bGoodLazySPath = FALSE;
    //     bGoodSpinPath = FALSE;
    //     bGoodDis = ComputeDisFromPointToLineCenter(ptStart, pTempPath, fDis);

    //     if (bGoodDis && fDis > MAX_AUTOLINE_DISTANCE)
    //     { // 20m
    //         bGoodDis = FALSE;
    //     }

    //     if (bGoodDis)
    //     {
    //         if (fabsf(fAngleDiff) > MAX_AUTOLINE_ANGLE) // 10~90 degree
    //         {
    //             nStartStep = STEP_NEED_SPIN;
    //             Posture ptNewStart;
    //             ptNewStart.fThita = lineAng.m_fRad; // forcet to parallel
    //             ptNewStart.GetPntObject() = ptStart.GetPntObject();
    //             bGoodLazySPath = SearchGoodLazySPath(ptNewStart, pTempPath, fTempRate, ptTempEnd);
    //         }
    //         else
    //         {
    //             bGoodLazySPath = SearchGoodLazySPath(ptStart, pTempPath, fTempRate, ptTempEnd);
    //             nStartStep = STEP_NEED_S_CURVE;
    //         }
    //     }

    //     if ((!bGoodLazySPath && !bGoodSpinPath) || !bGoodDis)
    //         continue;

    //     if (bGoodLazySPath)
    //     {
    //         if ((pathIndex == 0xFFFF) || (fDis < fMinDis) || (fabsf(fDis - fMinDis) <= 0.05 && (fTempRate < fMinRate)))
    //         {
    //             fMinRate = fTempRate;
    //             fMinDis = fDis;
    //             pathIndex = i;
    //             pPath = pTempPath;
    //             ptEnd = ptTempEnd;
    //             ang = pPath->GetHeading(nd);
    //             if (STEP_NEED_SPIN == nStartStep)
    //             {
    //                 nResult = STEP_NEED_SPIN;
    //                 bGoodSpinPath = SearchGoodSpinPath(ptStart, pTempPath, fTempRate, angStartTemp, angEndTemp, TurnDirTemp);
    //                 if (bGoodSpinPath)
    //                 {
    //                     angStart = angStartTemp;
    //                     angEnd = angEndTemp;
    //                     TurnDir = TurnDirTemp;
    //                     fSpinRad = fTempRate;
    //                 }
    //             }
    //             else
    //             {
    //                 nResult = STEP_NEED_S_CURVE;
    //             }
    //         }
    //     }
    // }
    // fLazySRate = fMinRate;
    // return nResult;
}

bool WorkStatusRecord::SearchGoodSpinPath(const Posture& start, Path* path, float& spin_radius,
                            Angle& start_angle, Angle& end_angle, TurnDir& turn_dir)
{
    return false;
    // if (pPath == NULL)
    // {
    //     return FALSE;
    // }
    // Node nd;
    // angStart = Angle(ptStart.fThita);
    // angEnd = Angle(pPath->GetHeading(nd));

    // Angle angTurn;
    // angTurn.m_fRad = angEnd.m_fRad - angStart.m_fRad;

    // angTurn.NormAngle(); //[0, 2 * PI) qf

    // TurnDir = (angTurn.Quadrant() <= 2) ? COUNTER_CLOCKWISE : CLOCKWISE;
    // if (angTurn.m_fRad > PI)
    // {
    //     angTurn.m_fRad = 2 * PI - angTurn.m_fRad; // clockwise
    // }
    // fSpinRad = angTurn.m_fRad;
    // return TRUE;
}
